#include "can_base.h"


#define MOTOR_ID 			1

float targetVotage = 0.0f;
float test1,test2 = 0;
uint32_t lastRecvTime = 0;
void CAN_Init(struct Can *can);


Can can = {

	.ID = 0x101,
	.trageValue = &targetVotage,
	
	.DriverInit = CAN_Init,

};

void CAN_Init(struct Can *can)
{
	CAN_FilterTypeDef filter;
	filter.FilterActivation = ENABLE;
	filter.FilterMode = CAN_FILTERMODE_IDMASK;
	filter.FilterMode = CAN_FILTERMODE_IDMASK;
	filter.FilterScale = CAN_FILTERSCALE_32BIT;
	filter.FilterIdHigh = 0x0000;
	filter.FilterIdLow = 0x0000;
	filter.FilterMaskIdHigh = 0x0000;
	filter.FilterMaskIdLow = 0x0000;
	filter.FilterBank = 0;
	filter.FilterFIFOAssignment = CAN_RX_FIFO0;
	HAL_CAN_ConfigFilter(&hcan, &filter);
	HAL_CAN_Start(&hcan);
	HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING);
}


void CAN_SendState(float angle, float speed)
{
	CAN_TxHeaderTypeDef header;
	header.StdId = can.ID;
	header.IDE = CAN_ID_STD;
	header.RTR = CAN_RTR_DATA;
	header.DLC = 8;
	
	uint8_t data[8];
	memcpy(data,&angle,4); 
	memcpy(&data[4],&speed,4); 

	
	uint32_t mailbox;
	HAL_CAN_AddTxMessage(&hcan, &header, data, &mailbox);
}
//void CAN_SendState_my(uint16_t _data, float speed)
//{
//	CAN_TxHeaderTypeDef header;
//	header.StdId = can.ID;
//	header.IDE = CAN_ID_STD;
//	header.RTR = CAN_RTR_DATA;
//	header.DLC = 4;
//	
//	uint8_t data[4];
//	memcpy(data,&_data,2); 
//	memcpy(&data[2],&(int16_t){speed*10},2); 

//	
//	uint32_t mailbox;
//	HAL_CAN_AddTxMessage(&hcan, &header, data, &mailbox);
//}




void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef header;
	uint8_t rxData[8];
	
	if(HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &header, rxData) != HAL_OK)
		return;
	
	if(header.StdId == 0x100 && MOTOR_ID <= 2) 
	{
		targetVotage = *(float*)&rxData[(MOTOR_ID-1)*4];
		lastRecvTime = HAL_GetTick();
	}
	else if(header.StdId == 0x200 && MOTOR_ID > 2) 
	{
		targetVotage = *(float*)&rxData[(MOTOR_ID-3)*4];
		lastRecvTime = HAL_GetTick();
	}
	
	
}

uint8_t Motor_OfflineCheckProcess(void)
{
	static uint8_t isOffline = 0;
	if(HAL_GetTick() - lastRecvTime > 500)
	{
		if(!isOffline)
		{
			
			targetVotage = 0;
			isOffline = 1;
			HAL_GPIO_WritePin(LDE_ERROR_GPIO_Port, LDE_ERROR_Pin, GPIO_PIN_SET);
		}
	}
	else
	{	isOffline = 0;
		HAL_GPIO_WritePin(LDE_ERROR_GPIO_Port, LDE_ERROR_Pin, GPIO_PIN_RESET);
	}
	
	return isOffline;
}

